/* Autostop
 *
 * Copyright (c) 2006 - 2007 Maciej Gajewski, Grzegorz Latarowski
 * http://code.google.com/p/autostop-automaty-drogowe
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 2 of the License, or (at your
 * option) any later version. NO WARRANTY OF ANY KIND is expressed or implied.
 */

// libc
#include <math.h>

// Qt
#include <QSettings>

// camera
#include "camera.h"

// autonet
#include "autopackets.h"

// local
#include "imageanalyzer.h"
#include "gexception.h"
#include "globaldata.h"
#include "constants.h"


#include <sys/time.h>
// ==================== getm - benchmarker
double getms()
{
	struct timeval tv;
	gettimeofday( &tv, NULL );
	
	return (tv.tv_sec & 0xff ) * 1e3 + tv.tv_usec * 1e-3;
}

// ======================== run =====================
void ImageAnalyzer::run()
{
	// intialzie
	init();
	GlobalData *pGlobal = GlobalData::instance();
	
	// analyze
	
	// allocate segmentd image buffer
	// NOTE this buffer is doomed to be memory leak. there is no point to control this
	int width	= camera_image_width() / 8;
	int height	= camera_image_height() / 8;
	pimage_t p_segmented = create_image( width, height );
	int bufferSize = width * height;
	if ( ! p_segmented )
	{
		throw GSysError( "Could not allocate segmentd image buffer" );
	}
	
	// udp stream params
	QHostAddress targetAddress;
	quint16 targetPort = 0;
	
	// udp-sending timestamps
	quint64 lastJpegTime	= 0;	// last time jpeg was sent
	
	//  Main loop - one iteration per frame
	while ( 1 )
	{
		int cameraResult		= CAMERA_OK;	// result returned by camera routines
		quint64 timestamp	= 0;				// timestamp
		
		// TODO debug
		//double startTime = getms();
		
		// read accel
		double accelX = camera_accel_get_x();
		double accelY = camera_accel_get_y();
		
		// check accel
		bool accelOk = fabs( accelX - _accelExpectedX ) <= _accelToleranceX
							&& fabs( accelY - _accelExpectedY ) <= _accelToleranceY;
		// TODO handle more smart
		if ( accelOk )
		{
			
			cameraResult = camera_get_segmented_image( p_segmented->pixels, bufferSize, &timestamp );
			
			// TODO test
			//printf("Segmented readed: %.2lf ms\n", getms() - startTime );
			
			if ( CAMERA_OK != cameraResult )
			{
				QString msg = QString("Error obtaining image from camera: %1")
						.arg( camera_error_message() );
				throw GConfigurationError( msg );
			}
	
			// Add image to process
			add_segmented_frame( _pProcess, p_segmented, timestamp );
	
			// Search process output for interesting moving object
			checkForInterestingObjects();
		}
		else
		{
			// remove all objects
			_pProcess->objects.empty();
			qDebug("Accel value exceeded");
			qDebug("Diff: %.04lf, %.04lf", accelX - _accelExpectedX, accelY - _accelExpectedY ); // TODO remove 
		}
		
		// check for incoming params
		if ( pGlobal->paramsNew() )
		{
			readParams( pGlobal->getParams() );
		}
		
		// check for new masks
		if ( pGlobal->masksNew() )
		{
			loadMasks();
			pGlobal->setNewMasks( false );
		}
		
		// check if address was updated
		if ( pGlobal->addressNew() )
		{
			targetAddress = pGlobal->getUdpAddress();
			targetPort = pGlobal->getUdpPort();
		}
		
		// is it time to send something, and is there someone listening ?
		if ( ! targetAddress.isNull() && targetPort > 0 )
		{
			// sent proper format
			if ( _image_format == PARAM_IMAGEFORMAT_JPG2K )
			{
				// time for jpeg?
				if ( timestamp - lastJpegTime > (int)_intervalJpeg && _intervalJpeg >= 0 )
				{
					sendJpeg( targetAddress, targetPort );
					
					lastJpegTime = timestamp;
				}
			}
			else if ( _image_format == PARAM_IMAGEFORMAT_SEGMENTED )
			{
				sendImage( targetAddress, targetPort, p_segmented );
			}
			// TODO test
			//printf("FRAME sent: %.2lf ms\n", getms() - startTime ); // TODO remove
		
			sendObjects( targetAddress, targetPort );
			
		}
		
		// TODO test
		//printf("Total time: %.2lf ms\n", getms() - startTime );
		
	} // main loop

	// basically... that's all, folks!

}

// ======================== checkForInterestingObjects =====================
void ImageAnalyzer::checkForInterestingObjects()
{

	for( list<object_t>::iterator objecti = _pProcess->objects.begin()
			; objecti != _pProcess->objects.end(); )
	{
		object_t& object = *objecti;
		bool deleteObject	= false;	// if delete object n this iteration
		//qDebug("obj: shuttertime: %d, speed: %lf"
		//	, (int)object.shutterTime, (double)object.speed);
		// if object has interesting speed, report it and remove from process
		if ( object.shutterTime > 0 && object.speed > 0 )
		{
			qDebug("object detected, speed: %.4lf/ %d",
					(double)object.speed, _max_speed );
		
			if ( object.speed > _max_speed && _max_speed > 0 )
			{
				reportObject( object );
			}
			
			// delete object
			deleteObject = true;
			
		} // shutter
		
		// delete object if needed
		if ( deleteObject )
		{
			objecti = _pProcess->objects.erase( objecti );
		}
		else
		{
			objecti++;
		}
		
		
	} // object loop
	
}

// ======================== init =====================
void ImageAnalyzer::init()
{
	// memebers init
	_showDiff			= false;
	_max_speed			= 0;
	_intervalJpeg		= 1000;
	_image_format		= PARAM_IMAGEFORMAT_JPG2K;
	_image_id			= 0;
	_showAllObjects		= false;
	_accelExpectedX		= 0.5;
	_accelExpectedY		= 0.5;
	_accelToleranceX	= 0.002;
	_accelToleranceY	= 0.002;

	// create anaalyzer process
	qDebug("Initializing process");
	_pProcess = create_process();
	
	if ( ! _pProcess )
	{
		throw GConfigurationError("Error creating process");
	}
	
	loadMasks();
	
	// init camera device
	if ( CAMERA_ERROR == camera_init() )
	{
		QString msg = QString( "Error initilizing camera: %1" ).arg( camera_error_message() );
		throw GConfigurationError( msg );
	}
	// trun off auto exposure, set 60ms exposure time, set jpeg shrink
	camera_set_auto_exposure( 0 );
	camera_set_exposure( 60 );
	camera_jpeg_set_shrink( 1 );
	
	// lopad params
	// TODO actually load
	_pProcess->reference_distance = 100000;	// 100m

	
	// NOTE I think this should be here.
		// load params
	GParams params;
	try
	{
		params.loadFromFile( PARAMS_CONFIG_FILE );
	}
	catch( const GException & e )
	{
		qWarning( "Exception in Server::initConfiguration(): %s",
				  qPrintable( e.getMessage() )
				);
		qWarning( "Tries to reload default params." );

		try
		{
			// get default values

				// load params description from default config file 
			GParamsDescription paramsDescription;
			paramsDescription.loadFromFile( CAMERA_CONFIG_FILE );
				// get params with default values
			params = paramsDescription.defaultParams();
				// save it
			params.saveToFile( PARAMS_CONFIG_FILE );
		}
		catch( const GException & e )
		{
			// TODO handle error
			qWarning( "Exception in Server::initConfiguration(): %s",
					qPrintable( e.getMessage() )
					);
		}
	}
		// write to global data
	GlobalData::instance()->setParams( params );
		// load this params 
	readParams( GlobalData::instance()->getParams() );
	
	// load settings
	loadSettings();
}

// ======================== loadSettings =====================
void ImageAnalyzer::loadSettings()
{
	QSettings settings( SETTINGS_FILE, QSettings::IniFormat );
	if ( settings.status() == QSettings::NoError )
	{
		// load settings
		settings.beginGroup("accelerometer"); // TODO use defined const
		_accelExpectedX		= settings.value( "ExpectedX", _accelExpectedX ).toDouble();
		_accelExpectedY		= settings.value( "ExpectedY", _accelExpectedY ).toDouble();
		_accelToleranceX	= settings.value( "ToleranceX", _accelToleranceX ).toDouble();
		_accelToleranceY	= settings.value( "ToleranceY", _accelToleranceY ).toDouble();
		
		// re-write settings
		settings.setValue( "ExpectedX", _accelExpectedX );
		settings.setValue( "ExpectedY", _accelExpectedY );
		settings.setValue( "ToleranceX", _accelToleranceX );
		settings.setValue( "ToleranceY", _accelToleranceY );
	}
	else
	{
		qWarning("Can not read settings file %s", SETTINGS_FILE );
	}
}

// ======================== loadMasks =====================
void ImageAnalyzer::loadMasks()
{
	// load markers
	FILE* fileMarkers = fopen( MARKERS_FILE, "r" );
	if ( fileMarkers )
	{
		free_image( _pProcess->pdistance_markers );
		_pProcess->pdistance_markers = load_image( fileMarkers );
		fclose( fileMarkers );
	}
	else
	{
		QString msg = QString("No markers file found (%1)").arg( MARKERS_FILE );
		throw GSysError( msg );
	}
	
	// load roi
	FILE* fileROI = fopen( ROI_FILE, "r" );
	if ( fileROI )
	{
		free_image( _pProcess->pROI );
		_pProcess->pROI = load_image( fileROI );
		fclose( fileROI );
	}
	else
	{
		QString msg = QString("No ROI file found (%1)").arg( ROI_FILE );
		throw GSysError( msg );
	}
	
	// load shutter image
	
	FILE* fileShutter = fopen( SHUTTER_FILE, "r" );
	if ( fileShutter )
	{
		free_image( _pProcess->pshutter_markers );
		_pProcess->pshutter_markers = load_image( fileShutter );
		fclose( fileShutter );
	}
	else
	{
		QString msg = QString("No shutter file found (%1)").arg( SHUTTER_FILE );
		throw GSysError( msg );
	}
	
}

// ======================== reportObject =====================
void ImageAnalyzer::reportObject( const object_t& object )
{
	// TODO remove debug
	qDebug("reporting object");
	
	//send only if 2d cam address defined
	if ( ! _secondCameraAddr.isNull() )
	{
		qDebug("sending udp packet, times: %d, %d", (int)object.shutterTime, (int)camera_get_time());
		GAutoEventTrigger packet( GAutoEventTrigger::EVENT_SPEEDING );
		
		packet.setMessage( QString(tr("Speeding car detected, speed: %1")).arg( object.speed ) );
		packet.setShutterTime( object.shutterTime );
		packet.setSendTime( camera_get_time() );
	
		sendUdpPacket( packet, _secondCameraAddr, 34445 );	// TODO use defined constant
	}
}

// ======================== readParams =====================
void ImageAnalyzer::readParams( const GParams & params )
{
	if ( _pProcess )
	{
		const char* bgfactor			= PARAM_BGFACTOR;
		const char* fgtreshold			= PARAM_FGTRESHOLD;
		const char* maxgroupdistance	= "maxgroupdistance";
		const char* diff				= "diff";
		const char* roi					= "roi";
		const char* refdistance			= PARAM_REFERENCE_DISTANCE;
		
		// bgfactor
		if ( params.contains( bgfactor ) )
		{
			double background_factor = params.value( bgfactor ).toDouble();
			_pProcess->params.background_factor = background_factor;
			// this line od got from analyzer insisde. This is manipulating on private 
			// data, this cis ugly!
			_pProcess->_int_background_factor = (int)( (1<<16) * background_factor );
			// also set into camera
			camera_set_bgfactor( (int)(background_factor * 255) ); // NOTE there are two, uncompatible values!
// 			qDebug("bgfactor set to %lf (%d)"
// 				, _pProcess->params.background_factor
// 				, _pProcess->_int_background_factor
// 				 );
		}
		
		// treshold
		if ( params.contains( fgtreshold ) )
		{
			double treshold = params.value( fgtreshold ).toDouble();
			int tresholdInt = int( 255 * treshold );
			_pProcess->params.foreground_treshold = tresholdInt;
			camera_set_fgtreshold( tresholdInt );
// 			qDebug("fgtreshold to %.2f (%d)", treshold, tresholdInt );
		}
		
		// max distance
		if ( params.contains( maxgroupdistance ) )
		{
			_pProcess->params.max_group_distance = params.value( maxgroupdistance ).toInt();
// 			qDebug("maxgroupdistance to %d", (int)_pProcess->params.max_group_distance );
		}
		
		// show difference
		if ( params.contains( diff ) )
		{
			_showDiff = params.value( diff ).toBool();
// 			qDebug("diff to %d", (int)_showDiff );
		}
		
		// refdisyance
		if ( params.contains( refdistance ) )
		{
			_pProcess->reference_distance = params.value( refdistance ).toInt();
		}
		
		// show roi
		if ( params.contains( roi ) )
		{
			_showRoi = params.value( roi ).toBool();
		}
		
		// jpeg interval
		if ( params.contains( PARAM_JPEG_INTERVAL ) )
		{
			_intervalJpeg = params.value( PARAM_JPEG_INTERVAL ).toInt();
		}
		// exposure
		if ( params.contains( PARAM_EXPOSURE ) )
		{
			camera_set_exposure( params.value( PARAM_EXPOSURE ).toInt() );
		}
		// jpeg size
		if ( params.contains( PARAM_JPEG_SIZE ) )
		{
			camera_jpeg_set_quality( params.value( PARAM_JPEG_SIZE ).toInt() );
		}
		// image format
		if ( params.contains( PARAM_IMAGEFORMAT ) )
		{
			_image_format = params.value( PARAM_IMAGEFORMAT ).toInt();
		}
		// min object size
		if ( params.contains( PARAM_MIN_OBJ_SIZE ) )
		{
			_pProcess->params.min_group_size = params.value( PARAM_MIN_OBJ_SIZE ).toInt();
		}
		// max object size
		if ( params.contains( PARAM_MAX_OBJ_SIZE ) )
		{
			_pProcess->params.max_group_size = params.value( PARAM_MAX_OBJ_SIZE ).toInt();
		}
		// show all ojects
		if ( params.contains( PARAM_SHOW_ALL_OBJECTS ) )
		{
			_showAllObjects = params.value( PARAM_SHOW_ALL_OBJECTS ).toBool();
		}
		// 2nd cam address
		if ( params.contains( PARAM_CO_CAMERA_IP ) )
		{
			_secondCameraAddr.setAddress( params.value( PARAM_CO_CAMERA_IP ).toString() );
		}
		// max allowed speed
		if ( params.contains( PARAM_MAX_SPEED ) )
		{
			_max_speed = params.value( PARAM_MAX_SPEED ).toInt();
		}
		
	}
	
	// TEST - turn off auto once again
	camera_set_auto_exposure( 0 );
}

// ======================== prepareDiff =====================
/// Fill image with its own difference from process backgorund
void ImageAnalyzer::prepareDiff( pimage_t pImage )
{
	if ( _pProcess )
	{
		pixel_t* pBackgroundPixel = _pProcess->pbackground->pixels;
		foreach_pixel( pPixel, pImage )
		{
			*pPixel = abs( *pPixel - *pBackgroundPixel );
			pBackgroundPixel++;
		}
	}
}

// ======================== addRoi =====================
/// Sets all images outside provcess's ROI to gray
void ImageAnalyzer::addRoi( pimage_t pImage )
{
	if ( _pProcess )
	{
		pixel_t* pRoiPixel = _pProcess->pROI->pixels;
		foreach_pixel( pPixel, pImage )
		{
			if ( *pRoiPixel == 0 )
			{
				*pPixel = 128;
			}
			pRoiPixel ++;
		}
	}
}

// ======================== sendJpeg =====================
/// Gets jpeg from camera and sends it  via UDP
void ImageAnalyzer::sendJpeg( const QHostAddress& address, quint16 port )
{
	// get jpeg
	const int bufferSize = 1300 * camera_get_jpeg_quality(); 
	QByteArray buffer;
	buffer.resize( bufferSize );
	
	size_t jpegSize = 0;
	if ( CAMERA_OK != camera_get_jpeg( (uint8_t*)buffer.data(), buffer.size(), &jpegSize, NULL /*timestamp*/ ) )
	{
		qWarning("Could not read jpeg: %s", camera_error_message() );
		return;
	}
	buffer.resize( jpegSize );
	
	// create packet
	GAutoFrame packet;
	packet.setImage( buffer );
	packet.setWidth( camera_image_width() ); // NOTE jpeg may have another size
	packet.setHeight( camera_image_height() );
	packet.setType( GAutoFrame::JPEG2K_YUV );
	packet.setId( _image_id ++ );
	
	// send packet
	sendUdpPacket( packet, address, port );
	/**/
}
	
// ======================== sendObjects =====================
/// Gets objects from process and sends them via UDP
void ImageAnalyzer::sendObjects( const QHostAddress& address, quint16 port )
{
	// check process
	if ( ! _pProcess )
	{
		qWarning("ImageAnalyzer::sendObjects: null process");
		return;
	}
	
	// prepare packet
	GAutoObjectCoordinates packet;
	
	
	for( int i = 0; i < (int)_pProcess->groups.size(); i++ )
	{
		group_t group = _pProcess->groups[ i ];
		
		// is group 'alive'
		if ( group.pixels > 0 )
		{
			int scale = 1 << _pProcess->params.segmentation_scale_log; // groups are in 8 times lesser scale
			
			// calculate group size
			int width = ( group.maxx - group.minx + 1 ) * scale; // +1 to get outer edge
			int height = ( group.maxy - group.miny + 1 ) * scale;
			
			// is group interesting?
			bool interesting = qMin( width, height ) > _pProcess->params.min_group_size
									&& qMax( width, height ) < _pProcess->params.max_group_size;
			
			if ( interesting || _showAllObjects )
			{
				QString name = QString("(%1, %2) (%3 x %4)")
					.arg( group.mx * scale ).arg( group.my * scale )
					.arg( width ).arg( height );
				
				int mx = (int)( ( group.mx + 0.5 ) * scale );
				int my = (int)( ( group.my + 0.5 ) * scale );
				
				packet.append( QRect( group.minx * scale, group.miny * scale, width, height )
							, QPoint( mx, my )
							, name,
							QList< QPair< QPoint, qint16 > >()
							);
			}
		}
	}
	sendUdpPacket( packet, address, port );
}

// ======================== sendPacket =====================
void ImageAnalyzer::sendUdpPacket( const GAutoPacket& packet, const QHostAddress& address, quint16 port )
{
	QByteArray buffer;
	QDataStream stream( &buffer, QIODevice::WriteOnly );
	
	GHeader header = packet.header();
	
	stream << header << packet;
	
	_udpSender.sendSync( buffer, address, port );	
}


// ======================== sendImage =====================
void ImageAnalyzer::sendImage( const QHostAddress& address, quint16 port, pimage_t pImage )
{
	// TODO check pointer
	Q_ASSERT( pImage );
	
	
	int width = pImage->width;
	int height = pImage->height;
	/* TODO funny debug code - dumping image onto console
	qDebug("sending image %d x %d", width, height ); // TODO remove debug	
	// let's have a peek at the image
	int treshold = 100;
	for ( int y = 0; y < height; y++)
	{
		printf("\n");
		for( int x = 0; x< width; x++ )
		{
			printf("%c", pImage->pixels[ x + y*width ] > treshold ? 'x' : '.' );
		}
	}
	*/
	
	GAutoFrame packet;
	packet.setImage( QByteArray::fromRawData( (const char*)pImage->pixels, width*height ) );
	packet.setWidth( width );
	packet.setHeight( height );
	packet.setType( GAutoFrame::RAW_IMAGE );
	packet.setId( _image_id ++ );
	
	sendUdpPacket( packet, address, port );
}

// eof

